#ifndef __UTILITY_FUNCTIONS__
#define __UTILITY_FUNCTIONS__

#include <iostream>
template <int d, typename position, typename index, typename element, 
  typename ordering, typename policy>
void
sift_down2(position a, index i, element x, const ordering& less, policy& p) {
  //std::cout << " - before sift_down i,x: " << i << " " << x << std::endl;
  //show(a, a + p.size());
  //show(p.B.begin(), p.B.end());
  
  index j;
  while (p.last_child(i) <= p.last_leaf()) {
    j = p.top_all_present(a, i, less);
    if (less(x, *(a + j))) {
      p.update_all_present(a, i, *(a + j), less);
    }
    else {
      p.update_all_present(a, i, x, less);
      return;
    }
    i = j;
  }
  
  while(p.first_child(i) <= p.last_leaf()) {
    if(p.last_child(i) <= p.last_leaf())
      j = p.top_all_present(a, i, less);	
    else 
      j = p.top_some_absent(a, i, less);
    if (less(x, *(a + j))) {
      p.update_all_present(a, i, *(a + j), less);
      i = j;
    }
    else {
      p.update_all_present(a, i, x, less);
      return;
    }
  }    
  p.update_all_present(a, i, x, less);
  return;  


  //std::cout << "- after sift_down: " << std::endl;
  //show(a, a + p.size());
  //show(p.B.begin(), p.B.end()); 

}


template <int d, typename position, typename index, typename element, 
  typename ordering, typename policy>
void
sift_up(position a, index i, element x, const ordering& less, policy& p) {
  //std::cout << " - before sift_down i,x: " << i << " " << x << std::endl;
  //show(a, a + p.size());
  //show(p.B.begin(), p.B.end());
  
  position ak;
  while(1) {
    index m = (i / 3) * 3;
    ak = a + m;
    if(less(x, *(ak))) {
      *(a + i) = x;
      return;
    }
    else {
      if(m != 0) {
	m = ((m / 3) - 1) >> 2;
	i = ((2 * m) + 1 + (i / 3)) >> 1;
	m = (i + ((i / 3) * 3) - 1) >> 1;
	*(ak) = *(a + i);
	if(less(x, *(a + i))) {
	  *(ak) = x;
	  return;
	}
	*(a + i) = *(a + m);
      }
      else {
	*(ak) = x;
	return;
      } 
    }
  }
}


template <int d, typename position, typename index, typename element, 
  typename ordering, typename policy>
void
sift_down1(position a, index i, element x, const ordering& less, policy& p) {
  //std::cout << " - before sift_down i,x: " << i << " " << x << std::endl;
  //show(a, a + p.size());
  //show(p.B.begin(), p.B.end());

  index j = (i / 3) * 3;
  
  if((j != i) && (p.last_leaf() >= (j + 2))) {
    index l = ((((p.last_leaf() / 3) - 1) / 3) + 1) * 3;
    if(i >= l && (i > (j + 0)))
      return;
    index k = j + 1;
    while (i < k) {
      j = p.top_all_present(a, i, less);
      if(less(x, *(a + j))) 
	*(a + i) = *(a + j);    
      else {
	*(a + i) = x;
	return;
      }
      i = j;
    }
    if(i > l) {
      *(a + i) = x;
      return;
    }
    else {
      if(p.last_child(i) <= p.last_leaf())
	j = p.top_all_present(a, i, less);
      else {
	if(p.first_child(i) <= p.last_leaf())
	  j = p.top_some_absent(a, i, less);
	else {
	  *(a + i) = x;    
	  return;
	}
      }
      if  (less(x, *(a + j))) 
	*(a + i) = *(a + j);    
      else {
	*(a + i) = x;
	return;
      }
      i = j;
    }
  }

  index m = (p.last_leaf() / 12) * 3;
  while (i < m) {
    j = less(*(a + i + 1), *(a + i + 2)) ? i + 2 : i + 1;
    *(a + i) = *(a + j);
    i = ((j - (j / 3)) * 6); 
    if(less(*(a + i), *(a + i - 3))) i -= 3;
    if(less(*(a + i), x)) {
      if(less(*(a + j), x))
	*(a + (j / 3) * 3) =  x;
      else
	*(a + j) = x;	
      return;
    }
    else
      *(a + j) =  *(a + i);      	 
  }		  
  if ((((12 * (i / 3)) + 3) > p.last_leaf()) && ((i + 2) <= p.last_leaf())) {
    position ak = a + i;
    i = (less(*(a + i + 1), *(a + i + 2))) ? 2 : 1;
    if(less(*(ak + i), x)) {
      *(ak) = x;
      return;
    }
    *(ak) = *(ak + i);
    *(ak + i) = x;
    return;
  }
  
  if((i != 0) && (m == i)) {

    index k = i + 1;
    while (i < k) {
      j = p.top_all_present(a, i, less);
      if(less(x, *(a + j))) 
	*(a + i) = *(a + j);    
      else {
	*(a + i) = x;
	return;
      }
      i = j;
    }
    if(p.last_child(i) <= p.last_leaf())
      j = p.top_all_present(a, i,  less);
    else {
      if(p.first_child(i) <= p.last_leaf())
	j = p.top_some_absent(a, i, less);
      else {
	*(a + i) = x;
	return;
      }
    }
    if(less(x, *(a + j))) 
      *(a + i) = *(a + j);    
    else {
      *(a + i) = x; 
      return;
    }
    i = j;
  }
  
  sift_down2<d, position, index, element, ordering, policy>(a, i, x, less, p);
   
   

  //std::cout << "- after sift_down: " << std::endl;
  //show(a, a + p.size());
  //show(p.B.begin(), p.B.end()); 

}

template <int d, typename position, typename index, typename element, 
  typename ordering, typename policy>
void
sift_down(position a, index i, element x, const ordering& less, policy& p) {
  //std::cout << " - before sift_down i,x: " << i << " " << x << std::endl;
  //show(a, a + p.size());
  //show(p.B.begin(), p.B.end());

  index j;

  index m = (p.last_leaf() / 12) * 3;
  while (i < m) {
    m = i;
    j = less(*(a + i + 1), *(a + i + 2)) ? i + 2 : i + 1;
    *(a + i) = *(a + j);
    i = ((j - (j / 3)) * 6); 
    if(less(*(a + i), *(a + i - 3))) i -= 3;
      *(a + j) =  *(a + i);      	 
  }		  
  if ((((12 * (i / 3)) + 3) > p.last_leaf()) && ((i + 2) <= p.last_leaf())) {
    m = i;
    position ak = a + i;

    i = (less(*(a + i + 1), *(a + i + 2))) ? 2 : 1;

    *(ak) = *(ak + i);

    m = m + i;
    sift_up<d, position, index, element, ordering, policy>(a, m, x, less, p);
    return;
  }
		  

  if((i != 0) && (m == i)) {

    index k = i + 1;
    while (i < k) {
      j = p.top_all_present(a, i,  less);
      *(a + i) = *(a + j);    
      i = j;
    }
    if(p.last_child(i) <= p.last_leaf())
      j = p.top_all_present(a, i,  less);
    else {
      if(p.first_child(i) <= p.last_leaf())
	j = p.top_some_absent(a, i, less);
      else {
	sift_up<d, position, index, element, ordering, policy>(a, i, x, less, p);
	return;
      }
    }
    *(a + i) = *(a + j);    
    i = j;
  }
  

  if(less(*(a + i), x) && (i)) {
    m = ((i / 3) - 1) >> 2;
    i = ((2 * m) + 1 + (i / 3)) >> 1;
    m = (i + ((i / 3) * 3) - 1) >> 1;
    *(a + i) = *(a + m);
    sift_up<d, position, index, element, ordering, policy>(a, i, x, less, p);
    return;
  }


  sift_down2<d, position, index, element, ordering, policy>(a, i, x, less, p);

      
  //std::cout << "- after sift_down: " << std::endl;
  //show(a, a + p.size());
  //show(p.B.begin(), p.B.end()); 

}
 

#endif // __UTILITY_FUNCTIONS__


